#include "RandomWalk.h"
#include "../../utilities/Constants.h"
#include <stdlib.h>
#include <time.h>

#define QUEUE_SIZE 1000 // ROS topic queue size
#define INIT_DELAY 1 // delay (seconds) to initialize system
#define SUBS_DELAY 2 // delay (seconds) to apply subscriptions
#define MIN_DIST 0.3 // minimal distance (meters) to an obstacle
#define RAND_ANGLE_RANGE 10 // used to map randomly generated values to desired range of turn angles (radians)
#define BINARY_NUM 2 // modulo to choose between 0 or 1
#define TIME_FOR_TURN 1000000 // for how long (microseconds) a turn should be published

#define LEFT 1 // sign for left turning angle
#define RIGHT -1 // sign for right turning angle
#define DIFF_TH 0.1 // threshold of difference between samples (cm)- smaller differences are considered as equal

#define TOPIC_NAME "/Sensors_Module/Front_Laser_Range/RandomWalk"
#define TOPIC_SUFFIX "RandomWalk"

RandomWalk::RandomWalk(Driver* driver, const float& speed) {

	m_read_sensor = 0; // default is to consider sensor's data

	m_loop_rate = new ros::Rate(1000);

	m_driver = driver;
	m_speed = speed;

	m_east_bin = 0.0;
	m_north_east_bin = 0.0;
	m_north_bin = 0.0;
	m_north_west_bin = 0.0;
	m_west_bin = 0.0;

	// subscribe to sensors module
	m_sensors_publisher = m_node_handler.advertise<std_msgs::String>(
			"/Sensors_Module/Subscribers_Channel", 10);

	// subscribe to front laser
	sub_sensors_fl = m_node_handler.subscribe(TOPIC_NAME, QUEUE_SIZE,
			&RandomWalk::frontLaserCB, this);

	// force connection establishment
	m_loop_rate->sleep();
	sleep(INIT_DELAY);

	// subscribe to front laser via sensors node
	subscribeToLaser(true);

	// set seed for pseudo-random generator
	srand(time(NULL));
}

RandomWalk::~RandomWalk() {

	delete m_loop_rate;
	subscribeToLaser(false);
}

void RandomWalk::subscribeToLaser(bool shouldSubscribe) {

	ROS_INFO("in RandomWalk::subscribeToLaser");

	// compose message
	std_msgs::String m_message_to_publish;
	char buffer[128];
	snprintf(buffer, sizeof(buffer), "%s%s%d%s%d%s%f%s%f",
			TOPIC_SUFFIX, DELIMITER,
			sensors_names::FRONT_LASER_RANGE, DELIMITER, int(shouldSubscribe), DELIMITER,
			30.0, DELIMITER, 720.0);
	m_message_to_publish.data = buffer;

	// publish the message to sensors
	m_sensors_publisher.publish(m_message_to_publish);
	sleep(SUBS_DELAY);

	ROS_INFO("in RandomWalk::subscribeToLaser- published");
}

void RandomWalk::frontLaserCB(const std_msgs::Float32MultiArray::ConstPtr& range) {

	if (range->data.size() <= 0) {
		ROS_ERROR("RandomWalk::frontLaserCB data size = %d",range->data.size());
		return;
	}

	// calculate average distance for each bin
	float east = avgElements(range, 40, 50);

	float north_east = avgElements(range, 180, 200);
	float north = avgElements(range, 350, 370);
	float north_west = avgElements(range, 520, 540);

	float west = avgElements(range, 670, 680);

	// set the values safely
	setBins(east,north_east, north, north_west,west);

	// set all the range safely
	setRange(range);

	// check for emergency scenario
	if (m_read_sensor != 0) {
		isEmergencyStop();
	}
}

bool RandomWalk::isEmergencyStop() {

	// find minimal frontal distance
	float north_east, north, north_west;
	getBins(north_east, north, north_west);

	// check each direction
	if ((north_east <= MIN_DIST) || (north <= MIN_DIST) || (north_west <= MIN_DIST)) {

		ROS_ERROR("RandomWalk:: emergency stop");

		m_driver->stop();
		return true;
	}

	return false;
}

float RandomWalk::avgElements(
		const std_msgs::Float32MultiArray::ConstPtr& range, const int& start,
		const int& end) const {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range->data[i];
	}

	// return average
	return sum / (end - start + 1);
}

float RandomWalk::avgElements(const float range[], const int& start, const int& end) const {

	// sum elements
	float sum = 0.0;
	for (int i = start; i < end; ++i) {
		sum += range[i];
	}

	// return average
	return sum / (end - start + 1);
}

void RandomWalk::getBins(float& east,float& west) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	east = m_east_bin;
	west = m_west_bin;
	mlock.unlock();
}

void RandomWalk::getBins(float& north_east,float& north, float& north_west) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	north_east = m_north_east_bin;
	north = m_north_bin;
	north_west = m_north_west_bin;
	mlock.unlock();
}


void RandomWalk::getBins(float& east,float& north_east,float& north, float& north_west, float& west) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	east = m_east_bin;
	north_east = m_north_east_bin;
	north = m_north_bin;
	north_west = m_north_west_bin;
	west = m_west_bin;
	mlock.unlock();
}

void RandomWalk::setBins(const float east,const float north_east,const float north, const float north_west, const float west) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	m_east_bin = east;
	m_north_east_bin = north_east;
	m_north_bin = north;
	m_north_west_bin = north_west;
	m_west_bin = west;
	mlock.unlock();
}

void RandomWalk::setRange(const std_msgs::Float32MultiArray::ConstPtr& range) {

	std::unique_lock<std::mutex> mlock(m_mutex);
	for (int i = 0; i < MAX_RANGE_SIZE; ++i) {
		m_range[i] = range->data[i];
	}
	mlock.unlock();
}

void RandomWalk::chooseDirection() {

	m_read_sensor = 32000; // any big unsigned integer to indicate not to consider sensor's data

	// refresh data from callbacks
	ros::spinOnce();

	// choose a random turn angle
	int curr_rand = 20 + rand() % RAND_ANGLE_RANGE;
	float curr_angle = curr_rand / 10.0;

	// choose if should turn right or left
	short sign = chooseSign();

	ROS_ERROR("rand = %d, random angle = %f", curr_rand, sign * curr_angle);

	// turn in chosen direction
	m_driver->move(m_speed, sign * curr_angle);
	sleepAndWatch();

	// keep driving forward
	m_driver->move(m_speed, 0);

	m_read_sensor = 0;

	return;
}

short RandomWalk::chooseSign() {

	// get average distance at right and left edges
	float east, west;
	getBins(east, west);

	ROS_WARN("RandomWalk:chooseSign: e=%f, w=%f",east,west);

	// if both sides have almost the same value, choose one randomly
	if (std::abs(east - west) <= DIFF_TH) {

		int curr_rand = rand() % BINARY_NUM;
		return (curr_rand == 0) ? RIGHT : LEFT;
	}

	// turn to clearer side
	return (east >= west) ? RIGHT : LEFT;
}

void RandomWalk::sleepAndWatch() {

	for (int i = 0 ; i < 1000 ; ++i) {
		ros::spinOnce();
		usleep(TIME_FOR_TURN / 1000);
	}
}
